#!/usr/bin/env python3
"""
display_path_pyvista.py - 基于PyVista的全局路径可视化工具（优化版）
- 使用PyVista进行3D可视化，支持更丰富的显示效果
- 按顺序将路径点用直线连接起来
- 在每个坐标点旁边标注该点的顺序序号（优化显示数量）
- 优化处理速度，适合大量路径点的快速显示

功能特性:
- 路径点连线：高效连接所有路径点
- 序号标注：智能显示路径点序号（自动调节显示密度）
- 交互式3D显示：支持旋转、缩放、平移操作
- 高性能渲染：优化大数据量的显示性能
"""

import pyvista as pv
import numpy as np
import argparse
import os
from pathlib import Path


def parse_path_file(path_file):
    """解析路径文件，返回路径点列表 (x,y,z,rx,ry,rz)"""
    points = []
    
    if not os.path.exists(path_file):
        print(f"错误: 文件 {path_file} 不存在")
        return points
    
    try:
        with open(path_file, 'r') as f:
            for line_num, line in enumerate(f, 1):
                line = line.strip()
                if not line or line.startswith('#'):
                    continue
                
                # 尝试解析不同格式
                try:
                    if ',' in line:
                        # 逗号分隔格式
                        parts = [p.strip() for p in line.split(',')]
                    else:
                        # 空格分隔格式
                        parts = line.split()
                    
                    if len(parts) >= 6:
                        x, y, z, rx, ry, rz = map(float, parts[:6])
                        points.append([x, y, z, rx, ry, rz])
                    else:
                        print(f"警告: 第{line_num}行格式不正确，跳过")
                
                except ValueError as e:
                    print(f"警告: 第{line_num}行解析失败: {e}")
                    continue
    
    except Exception as e:
        print(f"错误: 读取文件失败: {e}")
        return []
    
    print(f"成功加载 {len(points)} 个路径点")
    return points





def visualize_path_with_pyvista(points):
    """使用PyVista可视化路径（优化版本）"""
    if not points:
        print("没有路径点可显示")
        return
    
    print("开始创建3D可视化...")
    
    # 创建PyVista绘图器
    plotter = pv.Plotter()
    plotter.set_background('white')
    
    # 提取坐标
    positions = np.array([[p[0], p[1], p[2]] for p in points])
    
    # 1. 显示路径点
    point_cloud = pv.PolyData(positions)
    plotter.add_mesh(point_cloud, color='blue', point_size=4, render_points_as_spheres=True)
    
    # 2. 高效创建路径连线
    if len(points) > 1:
        # 创建连续路径线 - 使用正确的PyVista线条格式
        n_points = len(positions)
        
        # 创建线条连接数组：[点数量, 点索引0, 点索引1, 点索引2, ...]
        lines = np.zeros((n_points + 1,), dtype=int)
        lines[0] = n_points  # 第一个元素是这条线包含的点数量
        lines[1:] = np.arange(n_points)  # 后续元素是点的索引
        
        # 创建线条数据
        line_data = pv.PolyData(positions)
        line_data.lines = lines
        plotter.add_mesh(line_data, color='gray', line_width=3)
    
    # 3. 批量添加序号标注（仅每隔几个点显示，提升性能）
    step = max(1, len(points) // 50)  # 最多显示50个标签
    labels_pos = []
    labels_text = []
    
    for i in range(0, len(points), step):
        point = points[i]
        x, y, z = point[:3]
        # 在点的稍微偏移位置添加文字标签
        offset = np.array([0.01, 0.01, 0.01])  # 小偏移
        label_pos = np.array([x, y, z]) + offset
        labels_pos.append(label_pos)
        labels_text.append(str(i + 1))
    
    # 批量添加标签
    if labels_pos:
        plotter.add_point_labels(labels_pos, labels_text, 
                                point_size=8, font_size=10, 
                                text_color='red', always_visible=True)
    
    # 设置相机和显示（简化设置以提升性能）
    plotter.add_axes()
    # 显示场景
    plotter.show()


def main():
    """主函数"""
    parser = argparse.ArgumentParser(description='PyVista路径可视化工具')
    parser.add_argument('--global-path', 
                       default='build/_global_sweep_path.txt',
                       help='全局路径文件路径')
    
    args = parser.parse_args()
    
    # 检查文件是否存在
    path_file = Path(args.global_path)
    if not path_file.exists():
        # 尝试在当前目录和build目录中查找
        possible_paths = [
            Path('_global_sweep_path.txt'),
            Path('build/_global_sweep_path.txt'),
            Path('../build/_global_sweep_path.txt')
        ]
        
        found = False
        for p in possible_paths:
            if p.exists():
                path_file = p
                found = True
                break
        
        if not found:
            print(f"错误: 找不到路径文件 {args.global_path}")
            print("请确保文件存在，或使用 --global-path 参数指定正确路径")
            return
    
    print(f"加载路径文件: {path_file}")
    
    # 解析路径文件
    points = parse_path_file(str(path_file))
    
    if not points:
        print("没有有效的路径点数据")
        return
    
    # 可视化
    visualize_path_with_pyvista(points)


if __name__ == "__main__":
    main()